package edu.oregonstate.vision.common;

import java.util.ArrayList;
import java.util.List;

import edu.oregonstate.vision.MatlabFacade;

import matlabcontrol.MatlabConnectionException;
import matlabcontrol.MatlabInvocationException;
import matlabcontrol.MatlabProxy;
import matlabcontrol.MatlabProxyFactory;
import matlabcontrol.extensions.MatlabNumericArray;
import matlabcontrol.extensions.MatlabTypeConverter;
import Jama.Matrix;

public class TanjentOrientationCalculator extends OrientationCalculator {

	private static TanjentOrientationCalculator orientationCalculator;

	public static TanjentOrientationCalculator getInstance() {
		if (orientationCalculator == null)
			orientationCalculator = new TanjentOrientationCalculator();
		return orientationCalculator;
	}

	private int neighbourhood = 5;

	private List<Point> findNearestPoints(Point point, List<Point> inputPoints) {
		List<Point> points = new ArrayList<Point>();
		Matrix distanceMatrix = EuclideanDistanceCalculator.getInstance()
				.calculate(inputPoints, inputPoints);
		points.add(point);
		for (int index = 0; index < neighbourhood; index++) {
			Point nearestPoint = null;
			for (Point p : inputPoints) {
				boolean flag = false;
				for (Point q : points) {
					if (p.equals(q))
						flag = true;
				}
				if (!flag) {
					if (nearestPoint == null
							|| point.distance(p) < point.distance(nearestPoint)) {
						nearestPoint = p;
					}
				}
			}
			points.add(nearestPoint);
			index++;
		}
		return points;
	}

	@Override
	public DescriptorMatrix calculate(List<Point> p, List<Point> q) {

		DescriptorMatrix matrix = new DescriptorMatrix(p.size(), q.size());

		return matrix;
	}

	@Override
	public DescriptorMatrix calculate(List<Point> points) {
		DescriptorMatrix matrix = new DescriptorMatrix(points.size(),
				points.size());
		// Create a proxy, which we will use to control MATLAB
		MatlabProxy proxy = null;
		try {
			proxy = MatlabFacade.getMatlabProxy();
			MatlabTypeConverter processor = new MatlabTypeConverter(proxy);
			for (Point point : points) {
				List<Point> nearestPoints = findNearestPoints(point, points);
				double[][] x = new double[1][nearestPoints.size()];
				double[][] y = new double[1][nearestPoints.size()];
				double[][] z = new double[1][nearestPoints.size()];

				for (int i = 0; i < nearestPoints.size(); i++) {
					x[0][i] = nearestPoints.get(i).getX();
					y[0][i] = nearestPoints.get(i).getY();
					z[0][i] = nearestPoints.get(i).getZ();
				}

				processor.setNumericArray("x", new MatlabNumericArray(x, null));
				processor.setNumericArray("y", new MatlabNumericArray(y, null));
				processor.setNumericArray("z", new MatlabNumericArray(z, null));
				proxy.eval("x = transpose(x)");
				proxy.eval("y = transpose(y)");
				proxy.eval("z = transpose(z)");
				proxy.eval("X = [x y zeros(size(x))]");
				proxy.eval("coefficient = X\\z;");
				// proxy.eval("plot3(x,y,z,'ro');grid on;hold on ");
				//
				// proxy.eval("[xx, yy]=meshgrid(x,y);");
				//
				// proxy.eval("zz = coefficient(1) * xx + coefficient(2) * yy + coefficient(3);");
				// proxy.eval("surf(xx,yy,zz);hold off");
				proxy.setVariable("x1", 1);
				proxy.setVariable("y1", 1);
				proxy.eval("z1 = coefficient(1) * x1 + coefficient(2) * y1 + coefficient(3);");

				proxy.setVariable("x2", 1);
				proxy.setVariable("y2", 2);
				proxy.eval("z2 = coefficient(1) * x2 + coefficient(2) * y2 + coefficient(3);");

				proxy.setVariable("x3", 2);
				proxy.setVariable("y3", 1);
				proxy.eval("z3 = coefficient(1) * x3 + coefficient(2) * y3 + coefficient(3);");

				proxy.eval("P1 = [x1,y1,z1]");
				proxy.eval("P2 = [x2,y2,z2]");
				proxy.eval("P3 = [x3,y3,z3]");
				proxy.eval("normal = cross(P1-P2, P1-P3)");
				double[] normal = ((double[]) proxy.getVariable("normal"));
				point.setOrthogonalX(normal[0]);
				point.setOrthogonalY(normal[1]);
				point.setOrthogonalZ(normal[2]);
			}

			for (int i = 0; i < points.size(); i++) {
				for (int j = 0; j < points.size(); j++) {
					double[][] v1 = new double[3][1];
					double[][] v2 = new double[3][1];
					v1[0][0] = points.get(i).getOrthogonalX();
					v1[1][0] = points.get(i).getOrthogonalY();
					v1[2][0] = points.get(i).getOrthogonalZ();
					v2[0][0] = points.get(j).getOrthogonalX();
					v2[1][0] = points.get(j).getOrthogonalY();
					v2[2][0] = points.get(j).getOrthogonalZ();
					processor.setNumericArray("v1", new MatlabNumericArray(v1,
							null));
					processor.setNumericArray("v2", new MatlabNumericArray(v2,
							null));
					proxy.eval("costheta = dot(v1, v2) / (norm(v1) * norm(v2));");
					proxy.eval("theta = acos(costheta)");
					matrix.set(i, j, ((double[]) proxy.getVariable("theta"))[0]);
				}
			}
		} catch (MatlabInvocationException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}

		return matrix;
	}
}
